//
//  PoseOptimiser.hpp
//  markerARLib
//
//  Created by CADCG on 16/4/28.
//  Copyright © 2016年 CvidKal. All rights reserved.
//

#ifndef PoseOptimiser_hpp
#define PoseOptimiser_hpp

#include <stdio.h>

#include "Convert.hpp"
#include "Frame.hpp"

using namespace std;

namespace markerAR
{
    

namespace PoseOptimiser
{
    typedef Eigen::Matrix3d SO3_type;
    typedef Eigen::Matrix<double,3,4> SE3_type;
    typedef Eigen::Vector3d so3_type;
    typedef Eigen::Matrix<double,1,6> se3_type;
    Eigen::Matrix<double, 3,3> skewSymmetricMatrix(const Eigen::Vector3d &v);
    SO3_type exp(const so3_type &so3);
    so3_type log(const SO3_type &SO3);
    SE3_type exp(const se3_type&se3);
    se3_type log(const SE3_type&se3);
    
//    class SE3
//    {
//        SE3(const Eigen::Matrix3d &R,const Eigen::Vector3d &t);
//        se3_type log();
//    private:
//        SE3_type dat;
//    };
    
    
    int PoseOptimiseByg2o(Frame* pf);
    int PoseOptimiseLM(Frame* pf);
    void testJacobian(Frame*pf);
    void PoseOptimiseByCeres(Frame*pf,bool verobose = false);
    void HomographyOptimiseByCeres(const vector<Eigen::Vector2d>&src,const vector<Eigen::Vector2d>&dst,Eigen::Matrix3d &H,std::vector<int>&mask,bool verbose=false);
}

}

#endif /* PoseOptimiser_hpp */
